import numpy as np
import mpctools as mpc
import scipy.linalg.interpolative as sli

def RANK_pzc(M):
    r = sli.estimate_rank(M, eps=1e-8) # eps越小秩相对越大; eps越小计算更快
#    svds = linalg.svd(M)
#    r = sum(svds > 1e-15)
#    U,Sigma,VT = np.linalg.svd(M)
#    r = np.linalg.matrix_rank(M)
    
    return r


alpha = 0


def SenMatrix(F,H,x,y,u,z,size):
    Nx = x.shape[1]
    Ny = y.shape[1]
    Nu = u.shape[1]
    Nz = z.shape[1]
    Sxx = np.zeros([size+1,Nx,Nx])
    Sxx[0,:,:] = np.eye(Nx)
    Syx = np.zeros([size+1,Ny,Nx])
    
    dFdx = np.zeros([size,Nx,Nx])
    dHdx = np.zeros([size+1,Ny,Nx])
        
    x_sen = np.zeros((size+1,Nx))
    z_sen = np.zeros((size,Nz))
    u_sen = np.zeros((size,Nu))
    
    x_sen = x
    u_sen = u
    z_sen = z

    for i in range(size):
        # Sensitivity matrix calculation
       JF = mpc.util.getLinearizedModel(F, [x_sen[i,:],z_sen[i,:],u_sen[i,:]],
                                         ["A","Z","B"],None)
       dFdx[i,:,:] = JF["A"]
        
       JH = mpc.util.getLinearizedModel(H, [x_sen[i,:]],["A"],None)
       dHdx[i,:,:] = JH["A"]
    
       Sxx[i+1,:,:] = mpc.mtimes(dFdx[i,:,:],Sxx[i,:,:])
       Syx[i,:,:] = mpc.mtimes(dHdx[i,:,:],Sxx[i,:,:])
    
    if size == 0:
        JH = mpc.util.getLinearizedModel(H, [x_sen[0,:]], ["A"],None)
        dHdx[0,:,:] = JH["A"]
        Syx[0,:,:] = mpc.mtimes(dHdx[0,:,:],Sxx[0,:,:])
    else:
        JH = mpc.util.getLinearizedModel(H, [x_sen[i+1,:]], ["A"],None)
        dHdx[i+1,:,:] = JH["A"]
        Syx[i+1,:,:] = mpc.mtimes(dHdx[i+1,:,:],Sxx[i+1,:,:])

    
    # Original sensitivity matrix
    for i in range(size+1):
        # Concatenate sensitivity matrixes
        if i==0:
            Sall = Syx[i,:,:]
        else:
            Sall = np.concatenate((Sall,Syx[i,:,:]), axis=0)
            
    return Sall



def SensAnal(Sall,Nx,Ny,sigma_w,sigma_v,size,rank_xp): # size is the value of window size in FIE and MHE
    rank = np.linalg.matrix_rank(Sall)#RANK_pzc(Sall)
    print(np.linalg.matrix_rank(Sall))
#    U,Sigma,VT = np.linalg.svd(Sall)
#    Slog = np.log10(Sigma)
#    print('log(Sigma)=',  Slog)
    rank_S = np.zeros([1,1],dtype=int)
    rank_S[0,0] = rank
    # Variable selection procudure
    Rl = np.zeros([Nx+1,(size+1)*Ny,Nx])
    Zl = np.zeros([Nx,(size+1)*Ny,Nx])
    SumCol = np.zeros([Nx,Nx])
    Flag = np.zeros([Nx,1],dtype=int)
    
    Rl[0,:,:] = Sall
    
    Xl = np.zeros([(size+1)*Ny,Nx])
    
    degalpha =  alpha*np.sqrt(sigma_w**2+sigma_v**2) #np.sqrt((size+1)*Ny) *
#    print('degalpha',degalpha)
    if rank_xp:
    #'''
    # Rank all
        for i in range(rank):
            for j in range(Nx):
                for k in range((size+1)*Ny):
                    SumCol[i,j] = SumCol[i,j] + Rl[i,k,j]**2
                SumCol[i,j] = np.sqrt(SumCol[i,j])
            Flag[i,0] = 0
            for i1 in range(Nx):
                if SumCol[i,Flag[i,0]] < SumCol[i,i1]:
                   Flag[i,0] = i1
                            
    # A prescribed value for sensitivity to terminate
 
            if SumCol[i,Flag[i,0]]  < degalpha:
                rank = i
                break
            else:
                Xl[:,i] = Sall[:,Flag[i,0]]
                                
    # select all variables or singular Xl: Terminate the selection
            if i == rank-1:
                break
            else:
                 rank_xx = np.linalg.matrix_rank(mpc.mtimes(Xl[:,0:i+1].T,Xl[:,0:i+1]))
                 if rank_xx == (i+1):
#                     print(Xl[:,0:i+1])
                     lsy=np.linalg.inv(mpc.mtimes(Xl[:,0:i+1].T,Xl[:,0:i+1]))
                     Zl[i,:,:] = mpc.mtimes(mpc.mtimes(mpc.mtimes(Xl[:,0:i+1],lsy),
                                            Xl[:,0:i+1].T),Sall)
                     Rl[i+1,:,:] = Sall-Zl[i,:,:]
    # Eliminate residual non-zero values
                     for i1 in range(i+1):
                         for i2 in range((size+1)*Ny):
                             Rl[i+1,i2,Flag[i1,0]] = 0
                 else:
                     rank = i+1
                     break
    maxcol = np.amax(SumCol,axis=1)
#    print('maxcol =',maxcol)
#    Js = functools.reduce(operator.mul, maxcol[0:rank], 1)
    Js = np.sum(maxcol)
#    Js = np.linalg.norm(maxcol)
####### if remove one sensor from set, the rank is not full: let degree = 0 to remain this sensor.      
#    if rank_S < Nx:
#        Js = Js*0

#    print('Js =',Js
#    print(rank) # rank: after orthogonolization; rank_S: calculate by RANK_pzc
    Result = np.concatenate((rank_S,Js),axis = None)  #,Flag,rank_S
    return Result